/*  ----------------------------------------------------------------------------
 *  Arello Mobile
 *  Fast 3d Engine
 *  ----------------------------------------------------------------------------
 * Except where otherwise noted, this work is licensed under
 * http://creativecommons.org/licenses/by/3.0
 */

#ifndef __LIBA_MATH_EIGEN_VECTORS_H__
#define __LIBA_MATH_EIGEN_VECTORS_H__

#include "matrix.h"
#include <cmath>

namespace liba
{

namespace math
{

template<typename T, int dim>
void find_eigen_values_and_vectors(const MatrixSquare<T, dim> & m, Vector<T, dim> * eigen_values, MatrixSquare<T, dim> * eigen_vectors)
{
	//NOTE: eigen vectors are returned in columns.

	//initialize method variables
	MatrixSquare<T, dim> Ak(m);
	MatrixSquare<T, dim> & V = *eigen_vectors;
	V = MatrixSquare<T, dim>::identity();
	
	for(int k = 0; k < 50; ++k)
	{
		//calc out-of-dialonal sum
		T out_diag_sum = 0;
		for(int i = 0; i < dim - 1; ++i)
			for(int j = i + 1; j < dim; ++j)
				out_diag_sum += Ak.rc(i, j) * Ak.rc(i, j);
		
		//if sum of out-diagolal elements is zero, the matrix is diagonal
		if(out_diag_sum == 0)break;
		
		//do rotations
		for(int the_i = 0; the_i < dim - 1; ++the_i)
		{
			for(int the_j = the_i + 1; the_j < dim; ++the_j)
			{
				//determine rotation angle
				T tau = (Ak.rc(the_j, the_j) - Ak.rc(the_i, the_i)) / (2. * Ak.rc(the_i, the_j));
				T sigma = 1. / (2. * tau);
				T abs_sigma = std::fabs(sigma);
				T t;
				if(abs_sigma >= 2.)
					t = (sigma > 0.) ? 1. : (sigma < 0.) ? -1. : 0.;
				else if(abs_sigma >= 1.)
					t = sigma / 2.;
				else if(abs_sigma >= .5)
					t = 2 * sigma / 3.;
				else t = sigma;
				
				T cos_phi = 1. / std::sqrt(1. + t * t);
				T sin_phi = t * cos_phi;
				
				//rotate the Ak matrix
				MatrixSquare<T, dim> Tm = MatrixSquare<T, dim>::identity();
				Tm.rc(the_i, the_i) = Tm.rc(the_j, the_j) = cos_phi;
				Tm.rc(the_i, the_j) = -sin_phi;
				Tm.rc(the_j, the_i) = sin_phi;

				MatrixSquare<T, dim> T_transponed = MatrixSquare<T, dim>::identity();
				T_transponed.rc(the_i, the_i) = T_transponed.rc(the_j, the_j) = cos_phi;
				T_transponed.rc(the_i, the_j) = sin_phi;
				T_transponed.rc(the_j, the_i) = -sin_phi;
			
				Ak = Tm * Ak * T_transponed;
				V = V * T_transponed;
			}
		}
	}

	Vector<T, dim> & e_value = *eigen_values;
	for(size_t i = 0; i < dim; ++i)
		e_value[i] = Ak.rc(i, i);
}

}//end of namespace math

}//end of namespace liba

using namespace liba;

#endif


